#include "motor.h"

#define PWM_FREQ_HZ     1000
#define PWM_RESOLUTION  LEDC_TIMER_8_BIT

MotorDriver::MotorDriver(int ain1, int ain2)
    : _ain1(ain1), _ain2(ain2),
      _ch_ain1(LEDC_CHANNEL_1), _ch_ain2(LEDC_CHANNEL_2)
{}

void MotorDriver::init()
{
    // 设置 PWM 定时器
    ledc_timer_config_t timer_conf = {};
    timer_conf.speed_mode       = LEDC_MODE;
    timer_conf.timer_num        = LEDC_TIMER_1;
    timer_conf.duty_resolution  = PWM_RESOLUTION;
    timer_conf.freq_hz          = PWM_FREQ_HZ;
    timer_conf.clk_cfg          = LEDC_AUTO_CLK;
    timer_conf.deconfigure      = false;  // ESP-IDF 5.0+ 新字段，必须赋值

    ledc_timer_config(&timer_conf);

    // 配置每个通道
	auto config_channel = [this](int gpio, ledc_channel_t ch) {
		ledc_channel_config_t channel = {};
		channel.gpio_num     = gpio;
		channel.speed_mode   = LEDC_MODE;
		channel.channel      = ch;
		channel.timer_sel    = LEDC_TIMER_1;
		channel.duty         = 0;
		channel.hpoint       = 0;
		channel.intr_type    = LEDC_INTR_DISABLE;

		ledc_channel_config(&channel);
	};

    config_channel(_ain1, _ch_ain1);
    config_channel(_ain2, _ch_ain2);

}

void MotorDriver::setMotor(ledc_channel_t ch1, uint8_t duty1,
                           ledc_channel_t ch2, uint8_t duty2)
{
    ledc_set_duty(LEDC_MODE, ch1, duty1);
    ledc_update_duty(LEDC_MODE, ch1);

    ledc_set_duty(LEDC_MODE, ch2, duty2);
    ledc_update_duty(LEDC_MODE, ch2);
}

void MotorDriver::stop()
{
    setMotor(_ch_ain1, 0, _ch_ain2, 0);

}

void MotorDriver::forward(uint8_t speed)
{
    setMotor(_ch_ain1, speed, _ch_ain2, speed);

}

void MotorDriver::backward(uint8_t speed)
{
    setMotor(_ch_ain1, speed, _ch_ain2, speed);
}

void MotorDriver::turnLeft(uint8_t speed)
{
    setMotor(_ch_ain1, speed, _ch_ain2, speed);
}

void MotorDriver::turnRight(uint8_t speed)
{
    setMotor(_ch_ain1, speed, _ch_ain2, speed);
}
